﻿using System;
using System.Collections.Generic;
using System.Text;
using LocalizationTest.Tools;
using System.Drawing;
using Cambia.CoreLib;

namespace LocalizationTest
{
  public class ContinuousAction2D : ActionDescription
  {
    public ContinuousAction2D()
    {
      action = new Vector2();
    }
    public ContinuousAction2D(Vector2 action)
    {
      this.action = action;
    }
    public Vector2 action;
    public override ActionDescription InvertAction()
    {
      return new ContinuousAction2D(new Vector2(-action.x, -action.y));
    }

    public override WorldState Execute(MotionModel motionModel, WorldState oldState)
    {
      if (Globals.selfLocatorProperties.MotionNoise == NoiseType.noNoise)
        return ExecutePerfect(motionModel, oldState);
      else
        return ExecuteWithError(motionModel, oldState);
    }

    public override WorldState ExecutePerfect(MotionModel motionModel, WorldState oldState)
    {
      double newPositionX = ((WorldState2D)oldState).Vector.x;
      double newPositionY = ((WorldState2D)oldState).Vector.y;

      return new WorldState2D(newPositionX + this.action.x , newPositionY + this.action.y);
    }

    private WorldState ExecuteWithError(MotionModel motionModel, WorldState oldState)
    {
      double newPositionX = ((WorldState2D)oldState).Vector.x;
      double newPositionY = ((WorldState2D)oldState).Vector.y;

      double sigma = 10;
      double gaussianErrorX = RandomProvider.NextNormal() * sigma; // Math.Sqrt(sigma);
      double gaussianErrorY = RandomProvider.NextNormal() * sigma; // Math.Sqrt(sigma);

      return new WorldState2D(newPositionX + this.action.x + gaussianErrorX, newPositionY + this.action.y + gaussianErrorY);
    }
    public override string ToString()
    {
      return
        this.GetType().Name + "|" +
        action.x.ToString() + ";" +
        action.y.ToString();
    }

    public override ActionDescription Parse(string str)
    {
//      string[] parts = str.Split('|');
      string[] parts2 = str.Split(';');
      return new ContinuousAction2D(new Vector2(double.Parse(parts2[0]), double.Parse(parts2[1])));
    }

  }

  public class ContinuousAction2DCreator
  {
    public Vector2 target;

    public double currentSpeed; //mm per frame
    public double currentRotationSpeed; //rad per frame

    double maxSpeed = 15;

    private Pose2D currentPose;

    public ContinuousAction2DCreator()
    {
      Reset(new Pose2D());
    }

    public ContinuousAction2DCreator(Pose2D initialPose)
    {
      Reset(initialPose);
    }

    public void Reset(Pose2D initialPose)
    {
      currentSpeed = 0;
      currentRotationSpeed = 0;
      currentPose = initialPose;
      target = new Vector2();
    }

    public Vector2 GetPositionDifference(WorldState2D simulatedWorldState)
    {
      currentPose.translation.x = simulatedWorldState.Vector.x;
      currentPose.translation.y = simulatedWorldState.Vector.y;
      currentRotationSpeed = Geometry.AngleTo(currentPose, target) / 2.0;

      double distanceToTarget = Geometry.DistanceTo(currentPose, target);
      if (distanceToTarget > 100 && currentSpeed < maxSpeed) currentSpeed += 1;
      if (distanceToTarget < maxSpeed * 2 ) currentSpeed = distanceToTarget / 2;

      currentSpeed = 1.5 * Globals.motionRasterWidth;

      DebugDrawingTool.CreateArrow("world:action creator:speed",
        currentPose.translation,
        currentPose.translation + new Vector2(Math.Cos(currentPose.rotation) * currentSpeed, Math.Sin(currentPose.rotation) * currentSpeed),
        4, Color.Red);

      Pose2D relativeChange = GetRelativePoseChangePerFrame();
      Pose2D oldPose = currentPose;
      currentPose += relativeChange;
      return currentPose.translation - oldPose.translation;
    }

    public Pose2D GetPoseDifference(WorldStatePose2D simulatedWorldState)
    {
      currentPose.translation.x = simulatedWorldState.Pose.translation.x;
      currentPose.translation.y = simulatedWorldState.Pose.translation.y;
      currentPose.rotation = simulatedWorldState.Pose.rotation;

      currentRotationSpeed = Geometry.AngleTo(currentPose, target) / 2.0;

      double distanceToTarget = Geometry.DistanceTo(currentPose, target);
      if (distanceToTarget > 100 && currentSpeed < maxSpeed) currentSpeed += 1;
      if (distanceToTarget < maxSpeed * 2) currentSpeed = distanceToTarget / 2;

      DebugDrawingTool.CreateArrow("world:action creator:speed",
        currentPose.translation,
        currentPose.translation + new Vector2(Math.Cos(currentPose.rotation) * currentSpeed, Math.Sin(currentPose.rotation) * currentSpeed),
        4, Color.Red);

      Pose2D relativeChange = GetRelativePoseChangePerFrame();
      Pose2D oldPose = currentPose;
      currentPose += relativeChange;
      return currentPose - oldPose;
    }

    private Pose2D GetRelativePoseChangePerFrame()
    {
      if (currentRotationSpeed == 0)
      {
        return new Pose2D(currentSpeed, 0, 0);
      }
      else
      {
        double numberOfFramesNeededForFullTurn = Math.PI * 2.0 / currentRotationSpeed;
        double circumferenceOfCircle = currentSpeed * numberOfFramesNeededForFullTurn;
        double radiusOfCirlce = circumferenceOfCircle / (Math.PI * 2.0);
        Pose2D start = new Pose2D();
        Pose2D end = new Pose2D(
          Math.Sin(currentRotationSpeed) * radiusOfCirlce,
          radiusOfCirlce - Math.Cos(currentRotationSpeed) * radiusOfCirlce,
          currentRotationSpeed);
        return end - start;
      }
    }
  }


  public class ContinuousAction2DCreatorOld
  {
    public double currentSpeed; //mm per frame
    public double currentRotationSpeed; //rad per frame

    private Pose2D currentPose;

    public ContinuousAction2DCreatorOld(Pose2D initialPose)
    {
      Reset(initialPose);
    }

    public ContinuousAction2DCreatorOld()
    {
      Reset(new Pose2D());
    }

    public void Reset(Pose2D initialPose)
    {
      currentSpeed = 0;
      currentRotationSpeed = 0;
      currentPose = initialPose;
    }

    public Vector2 GetPositionDifference()
    {
      DebugDrawingTool.CreateArrow("world:action creator:speed",
        currentPose.translation,
        currentPose.translation + new Vector2(Math.Cos(currentPose.rotation) * currentSpeed, Math.Sin(currentPose.rotation) * currentSpeed),
        4, Color.Red);

      Pose2D relativeChange = GetRelativePoseChangePerFrame();
      Pose2D oldPose = currentPose;
      currentPose += relativeChange;
      return currentPose.translation - oldPose.translation;
    }

    private Pose2D GetRelativePoseChangePerFrame()
    {
      if (currentRotationSpeed == 0)
      {
        return new Pose2D(currentSpeed, 0, 0);
      }
      else
      {
        double numberOfFramesNeededForFullTurn = Math.PI * 2.0 / currentRotationSpeed;
        double circumferenceOfCircle = currentSpeed * numberOfFramesNeededForFullTurn;
        double radiusOfCirlce = circumferenceOfCircle / (Math.PI * 2.0);
        Pose2D start = new Pose2D();
        Pose2D end = new Pose2D(
          Math.Sin(currentRotationSpeed) * radiusOfCirlce,
          radiusOfCirlce - Math.Cos(currentRotationSpeed) * radiusOfCirlce,
          currentRotationSpeed);
        return end - start;
      }
    }
  }
}
